﻿#include "speedtestwidget.h"

QSpeedTestWidget::QSpeedTestWidget(QWidget *parent) : QParamBasicWidget(parent)
{
    InitUI();

    QString sInfo;
    sInfo = sInfo + QString("当前KP:0.0\n");
    sInfo = sInfo + QString("当前KI:0.0\n");
    sInfo = sInfo + QString("前馈速度增益:0\n");
    sInfo = sInfo + QString("速度反馈滤波使能:未知\n");
    _pShowPIDLabel->setText(sInfo);

    //显示状态
    st_loopbackdata rcvdata;
    sInfo = getFrameStatus(rcvdata);
    _pShowStatusLabel->setText(sInfo);

    _pPlotDataTimer = new QTimer(this);
    connect(_pPlotDataTimer,&QTimer::timeout,this,[=](){plotPosData();plotSpeedData();plotCurrentData();});


    connect(_pStartBtn,&QPushButton::clicked,this,&QSpeedTestWidget::onStartBtnClicked);
    connect(_pStopBtn,&QPushButton::clicked,this,&QSpeedTestWidget::onStopBtnClicked);

    connect(_pQueryParamBtn,&QPushButton::clicked,this,&QSpeedTestWidget::onQueryParamBtnClicked);
    connect(_pSetParamBtn,&QPushButton::clicked,this,&QSpeedTestWidget::onSetParamBtnClicked);

    connect(_pSaveDataCheckBox,&QCheckBox::clicked,this,&QSpeedTestWidget::onSaveDataBtnClick);

    //默认选中方位轴
    _i32AixsNo = 0;
}

/**
* @brief
*
**/
void QSpeedTestWidget::InitUI()
{
    _pLogWidget = new QLogWidget(this);

    _pPosPlot = new QCustomPlot(this);

    _pSpeedPlot = new QCustomPlot(this);

    _pCurreentPlot = new QCustomPlot(this);

    setPlotStyle(_pSpeedPlot,"时间(ms)","速度值(rpm)");
    setPlotStyle(_pPosPlot,"时间(ms)","位置(度)");
    setPlotStyle(_pCurreentPlot,"时间(ms)","电流(A)");
    _pCurreentPlot->addGraph();//增加通道
    _pCurreentPlot->addGraph();//增加通道
    _pCurreentPlot->graph(0)->setName("Q相电流设定值");
    _pCurreentPlot->graph(1)->setName("Q相电流反馈值");
    _pCurreentPlot->graph(2)->setName("A相电流");
    _pCurreentPlot->graph(3)->setName("B相电流");

    QPen pen;
    pen.setColor(QColor(56,203,125));
    pen.setWidth(1);
    _pCurreentPlot->graph(2)->setPen(pen);


    pen.setColor(QColor(128,0,128));
    _pCurreentPlot->graph(3)->setPen(pen);





    _pParamWidget = createParamWidget();


    QGridLayout* playout = new QGridLayout(this);
    playout->setSpacing(0);

    playout->addWidget(_pSpeedPlot,0,0,1,1);
    playout->addWidget(_pPosPlot,1,0,1,1);
    playout->addWidget(_pCurreentPlot,2,0,1,1);



    playout->addWidget(_pParamWidget,0,1,4,1);
    playout->addWidget(_pLogWidget,3,0,1,1);

    playout->setColumnStretch(0,4);
    playout->setColumnStretch(1,1);

    playout->setRowStretch(0,2);
    playout->setRowStretch(1,2);
    playout->setRowStretch(2,2);
    playout->setRowStretch(3,1);

}

/**
* @brief
*
**/
QWidget* QSpeedTestWidget::createPIDWidget()
{
    QWidget* pwidget = new QWidget(this);
    QUIQss::setWidgetStyleSheet(pwidget);

    _pSpeedForwardCombox = new QComboBox(this);
    _pSpeedForwardCombox->addItem("禁止");
    _pSpeedForwardCombox->addItem("使能");
    _pSpeedForwardCombox->setMinimumHeight(30);


    _pQueryParamBtn = new QPushButton("查询参数",this);
    _pSetParamBtn = new QPushButton("设置参数",this);




    QLabel* plabel1 = new QLabel("控制器比例KP:",this);
    QLabel* plabel2 = new QLabel("控制器比例KI(Hz):",this);
    QLabel* pForwardGainlabel = new QLabel("前馈速度增益:",this);
    QLabel* pForwardEnablelabel = new QLabel("速度反馈滤波使能:",this);

    _pKPEdit = new QLineEdit("0",this);
    _pKIEdit = new QLineEdit("0",this);
    _pSpeedForwardGainEdit = new QLineEdit("0",this);
    QUIQss::setTxtQss(_pKPEdit, "#DCE4EC", "#1ABC9C");
    QUIQss::setTxtQss(_pKIEdit, "#DCE4EC", "#1ABC9C");
    QUIQss::setTxtQss(_pSpeedForwardGainEdit, "#DCE4EC", "#1ABC9C");




    int nRow = 0;
    QGridLayout* playout = new QGridLayout(pwidget);

    playout->addWidget(plabel1,nRow,0,1,1);
    playout->addWidget(_pKPEdit,nRow,1,1,3);
    nRow++;
    playout->addWidget(plabel2,nRow,0,1,1);
    playout->addWidget(_pKIEdit,nRow,1,1,3);
    nRow++;
    playout->addWidget(pForwardGainlabel,nRow,0,1,1);
    playout->addWidget(_pSpeedForwardGainEdit,nRow,1,1,3);
    nRow++;
    playout->addWidget(pForwardEnablelabel,nRow,0,1,1);
    playout->addWidget(_pSpeedForwardCombox,nRow,1,1,3);
    nRow++;
    playout->addWidget(_pQueryParamBtn,nRow,0,1,1);
    playout->addWidget(_pSetParamBtn,nRow,1,1,3);
    nRow++;


    return pwidget;

}

/**
* @brief
*
**/
QWidget* QSpeedTestWidget::createTestWidget()
{
    QWidget* pwidget = new QWidget(this);
    QUIQss::setWidgetStyleSheet(pwidget);


    _pSignalTypeComBox = new QComboBox(this);
    _pSignalTypeComBox->addItem("横值");
    _pSignalTypeComBox->addItem("方波");
    _pSignalTypeComBox->addItem("正弦波");
    _pSignalTypeComBox->setMinimumHeight(30);

    _pFreqEdit = new QLineEdit("0",this);
    _pFreqEdit->setToolTip("频率范围:[0,20]Hz");

    _pAmpEdit = new QLineEdit("0",this);
    _pAmpEdit->setToolTip("速度:[-10,10]°/s");


    QUIQss::setTxtQss(_pFreqEdit, "#DCE4EC", "#1ABC9C");
    QUIQss::setTxtQss(_pAmpEdit, "#DCE4EC", "#1ABC9C");


    _pStartBtn = new QPushButton("开始测试",this);
    _pStopBtn = new QPushButton("停止测试",this);

    _pSaveDataCheckBox = new QCheckBox("保存实时数据",this);

    QLabel* plabel = new QLabel("信号类型:",this);
    QLabel* plabel3 = new QLabel("信号频率(Hz):",this);
    QLabel* plabel4 = new QLabel("速度(rpm):",this);

    int nRow = 0;
    QGridLayout* playout = new QGridLayout(pwidget);

    playout->addWidget(plabel,nRow,0,1,1);
    playout->addWidget(_pSignalTypeComBox,nRow,1,1,3);
    nRow++;
    playout->addWidget(plabel3,nRow,0,1,1);
    playout->addWidget(_pFreqEdit,nRow,1,1,3);
    nRow++;
    playout->addWidget(plabel4,nRow,0,1,1);
    playout->addWidget(_pAmpEdit,nRow,1,1,3);
    nRow++;
    playout->addWidget(_pStartBtn,nRow,2,1,1);
    playout->addWidget(_pStopBtn,nRow,3,1,1);
    nRow++;
    playout->addWidget(_pSaveDataCheckBox,nRow,0,1,1);
    nRow++;

    return pwidget;

}

/**
* @brief
*
**/
QWidget* QSpeedTestWidget::createParamWidget()
{
    QWidget* pwidget = new QWidget(this);
    QUIQss::setWidgetStyleSheet(pwidget);

    _pShowPIDLabel = new QLabel(this);
    _pShowPIDLabel->setAlignment(Qt::AlignTop);
    _pShowPIDLabel->setStyleSheet("color:rgb(0,0,255);font-size:20px;font-family: 'Microsoft YaHei';");


    _pShowStatusLabel = new QLabel(this);
    _pShowStatusLabel->setAlignment(Qt::AlignTop);
    _pShowStatusLabel->setStyleSheet("color:rgb(0,0,255);font-size:20px;font-family: 'Microsoft YaHei';");

    QTabWidget* pTabwidget = new QTabWidget(this);

    pTabwidget->setTabPosition(QTabWidget::South);

    pTabwidget->addTab(createPIDWidget(),"控制参数");
    pTabwidget->addTab(createTestWidget(),"速度环测试");

    int nRow = 0;
    QGridLayout* playout = new QGridLayout(pwidget);

    playout->addWidget(pTabwidget,nRow,0,1,2);
    nRow++;
    playout->addWidget(_pShowPIDLabel,nRow,0,1,1);
    playout->addWidget(_pShowStatusLabel,nRow,1,1,1);
    nRow++;
//    playout->addItem(new QSpacerItem(10,400,QSizePolicy::Minimum,QSizePolicy::Maximum),nRow,0,1,1);
    playout->setRowStretch(0,1);
    playout->setRowStretch(1,2);


    return pwidget;
}

/**
* @brief
*
**/
void QSpeedTestWidget::setPlotStyle(QCustomPlot* pPlot,QString xName,QString yName)
{
    pPlot->xAxis->setLabel(xName);
    pPlot->yAxis->setLabel(yName);

//    pPlot->xAxis->setRange(0, 1024);//设置范围
//    pPlot->yAxis->setRange(0, 100);

    pPlot->setInteractions(QCP::iRangeDrag | QCP::iRangeZoom /*| QCP::iSelectPlottables*/ | QCP::iMultiSelect);

    pPlot->addGraph();//增加通道
    pPlot->addGraph();//增加通道

    pPlot->graph(0)->setName("设定值");
    pPlot->graph(1)->setName("反馈值");


    QPen pen;
    pen.setColor(QColor(0,0,255));
    pen.setWidth(1);
    pPlot->graph(0)->setPen(pen);


    pen.setColor(QColor(255,0,0));
    pPlot->graph(1)->setPen(pen);


    pPlot->legend->setVisible(true);
    pPlot->legend->setFont(QFont("Helvetica",9));

}




/**
* @brief
*
**/
void QSpeedTestWidget::onStartBtnClicked()
{
    if(_xdatalist.size() == 0)
    {
        _pSpeedPlot->graph(0)->data().data()->clear();//清除数据
        _pSpeedPlot->graph(1)->data().data()->clear();//清除数据
        _pPosPlot->graph(0)->data().data()->clear();//清除数据
        _pPosPlot->graph(1)->data().data()->clear();//清除数据
        _pCurreentPlot->graph(0)->data().data()->clear();
        _pCurreentPlot->graph(1)->data().data()->clear();
        _pCurreentPlot->graph(2)->data().data()->clear();
        _pCurreentPlot->graph(3)->data().data()->clear();
    }


    int l_i32SigalType = _pSignalTypeComBox->currentIndex();//信号类型
    int l_i32Freq = _pFreqEdit->text().toFloat()*10;//频率
    short l_i16Amp = _pAmpEdit->text().toFloat();


    fourchar l_data;
    l_data.i32data = 0x0;
    l_data.data[3] = (l_i32SigalType&0xff);

    l_data.data[2] = 0;
    l_data.data[1] = (l_i16Amp>>8);
    l_data.data[0] = (l_i16Amp&0xff);

    emit signal_startTest(_i32AixsNo,0x31,l_data.ui32data,0x0);


    _bStartTest = true;


    _pPlotDataTimer->start(500);

    _pLogWidget->onShowlog("开始测试..");

}
/**
* @brief
*
**/
void QSpeedTestWidget::onStopBtnClicked()
{
    _pPlotDataTimer->stop();
    emit signal_startTest(_i32AixsNo,0x12,0,0);//关使能
    _pStartBtn->setText("开始测试");
    _bStartTest = false;
    _i32FrameCnt = 0;

    _xdatalist.clear();
    _yspeedlist[0].clear();
    _yspeedlist[1].clear();
    _yposlist[0].clear();
    _yposlist[1].clear();

    _ycurrentlist[0].clear();
    _ycurrentlist[1].clear();

    _ycurrentAlist.clear();
    _ycurrentBlist.clear();
}


/**
* @brief
*
**/
void QSpeedTestWidget::plotPosData()
{
    if(_bStartTest == false)
    {
        return;
    }
//    _pPosPlot->graph(0)->setData(_xposdatalist,_yposlist[0]);
    _pPosPlot->graph(0)->setData(_xdatalist,_yposlist[1]);

    double lfxmax = *std::max_element(std::begin(_xdatalist), std::end(_xdatalist));
    double lfxmin = *std::min_element(std::begin(_xdatalist), std::end(_xdatalist));

    double lfymax0 = *std::max_element(std::begin(_yposlist[1]), std::end(_yposlist[1]));
    double lfymin0 = *std::min_element(std::begin(_yposlist[1]), std::end(_yposlist[1]));


    if((lfxmax) >5000)
    {
        lfxmin = lfxmax - 5000;
    }

    if(lfxmax < 5000)
    {
        lfxmax = 5000;
    }
    _pPosPlot->xAxis->setRange(lfxmin,lfxmax);//幅度数据
    _pPosPlot->yAxis->setRange(lfymin0,lfymax0);//幅度数据

    _pPosPlot->replot();
}
/**
* @brief
*
**/
void QSpeedTestWidget::plotSpeedData()
{
    if(_bStartTest == false)
    {
        return;
    }
    _pSpeedPlot->graph(0)->setData(_xdatalist,_yspeedlist[0]);
    _pSpeedPlot->graph(1)->setData(_xdatalist,_yspeedlist[1]);

    double lfxmax = *std::max_element(std::begin(_xdatalist), std::end(_xdatalist));
    double lfxmin = *std::min_element(std::begin(_xdatalist), std::end(_xdatalist));

    double lfymax0 = *std::max_element(std::begin(_yspeedlist[1]), std::end(_yspeedlist[1]));
    double lfymin0 = *std::min_element(std::begin(_yspeedlist[1]), std::end(_yspeedlist[1]));


    if((lfxmax) > 5000)
    {
        lfxmin = lfxmax - 5000;
    }

    if(lfxmax < 5000)
    {
        lfxmax = 5000;
    }
    _pSpeedPlot->xAxis->setRange((lfxmin),(lfxmax));//幅度数据
    _pSpeedPlot->yAxis->setRange(lfymin0,lfymax0);//幅度数据

    _pSpeedPlot->replot();
}


/**
* @brief
*
**/
void QSpeedTestWidget::plotCurrentData()
{
    if(_bStartTest == false)
    {
        return;
    }
    _pCurreentPlot->graph(0)->setData(_xdatalist,_ycurrentlist[0]);
    _pCurreentPlot->graph(1)->setData(_xdatalist,_ycurrentlist[1]);

    _pCurreentPlot->graph(2)->setData(_xdatalist,_ycurrentAlist);//A相电流
    _pCurreentPlot->graph(3)->setData(_xdatalist,_ycurrentBlist);//B相电流


    double lfxmax = *std::max_element(std::begin(_xdatalist), std::end(_xdatalist));
    double lfxmin = *std::min_element(std::begin(_xdatalist), std::end(_xdatalist));

    double lfymax0 = *std::max_element(std::begin(_ycurrentlist[1]), std::end(_ycurrentlist[1]));
    double lfymin0 = *std::min_element(std::begin(_ycurrentlist[1]), std::end(_ycurrentlist[1]));


    if((lfxmax) > 5000)
    {
        lfxmin = lfxmax - 5000;
    }

    if(lfxmax < 5000)
    {
        lfxmax = 5000;
    }
    _pCurreentPlot->xAxis->setRange((lfxmin),(lfxmax));//幅度数据
    _pCurreentPlot->yAxis->setRange(lfymin0,lfymax0);//幅度数据

    _pCurreentPlot->replot();
}

/**
* @brief
*
**/
void QSpeedTestWidget::setButtonStyleSheet(QPushButton* pBtn)
{
    pBtn->setStyleSheet(
                "QPushButton:checked { background-color: rgb(245,90,70);;color:white;}"
                "QPushButton:unchecked { background-color: rgb(200,200,200);;color:black;}"
                "QPushButton{background-color: rgb(200,200,200);padding:5px;border-radius:5px;color:black;"
                "font-size:18px;font-family: 'Microsoft YaHei';}");


//    pBtn->setSizePolicy(QSizePolicy::Expanding,QSizePolicy::Expanding);
    pBtn->setMinimumHeight(20);
}

/**
* @brief 调试参数设置
*
**/
void QSpeedTestWidget::onQueryParamBtnClicked()
{
    emit signal_getpid(_i32AixsNo,0x21);//速度环路的比例增益
    emit signal_getpid(_i32AixsNo,0x22);//
    emit signal_getpid(_i32AixsNo,0x56);//
    emit signal_getpid(_i32AixsNo,0x57);//

}
/**
* @brief
*
**/
void QSpeedTestWidget::onSetParamBtnClicked()
{
    float lfKP = _pKPEdit->text().toFloat();
    float lfKI = _pKIEdit->text().toFloat();
    float lfForwardGain = _pSpeedForwardGainEdit->text().toFloat();

    int nIndex = _pSpeedForwardCombox->currentIndex();

    emit signal_setpid(_i32AixsNo,0x51,lfKP);
    emit signal_setpid(_i32AixsNo,0x52,lfKI);
    emit signal_setpid(_i32AixsNo,0x56,lfForwardGain);
    emit signal_setpid(_i32AixsNo,0x57,nIndex);

}

/**
* @brief
*
**/
void QSpeedTestWidget::onSaveDataBtnClick()
{
    if(_pSaveDataCheckBox->checkState() == Qt::Checked)
    {
        QString sFileFullPath = QDateTime::currentDateTime().toString("yyyyMMddhhmmss")+".csv";
        sFileFullPath = QApplication::applicationDirPath()+"/data/"+sFileFullPath;
        m_savefile.setFileName(sFileFullPath);
        writeDataToFile("时间,A相电流(A),B相电流(A),位置指令值,位置反馈值,速度指令,速度反馈,Q相电流指令(A),Q相电流反馈(A)\r");
        _i32SaveFrameCnt = 0;
        _bStartSaveData = true;

    }
    else
    {
        _bStartSaveData = false;
        writeDataToFile(m_sSaveDataInfo);
        m_sSaveDataInfo = "";
        m_savefile.close();
    }
}


/**
* @brief
*
**/
void QSpeedTestWidget::onRcvPidParam(QVariant var)
{
    st_pid_frame l_st_frame = var.value<st_pid_frame>();
    parsePIDParam(l_st_frame);

    QString sInfo;
    sInfo = sInfo + QString("当前KP:%1\n").arg(m_st_speed_pid.lfKP);
    sInfo = sInfo + QString("当前KI:%1\n").arg(m_st_speed_pid.lfKI);
    sInfo = sInfo + QString("前馈速度增益:%1\n").arg(_lfSpeedForwardGain);

    QString sEnable = _i32SpeedForwardEnable == 0?"禁止":"使能";
    sInfo = sInfo + QString("速度反馈滤波使能:%1\n").arg(sEnable);

    _pShowPIDLabel->setText(sInfo);
}

/**
* @brief
*
**/
void QSpeedTestWidget::rcvFrameData(st_loopbackdata rcvdata)
{
    if(_bStartTest == false)
    {
        return;
    }

    _i32FrameCnt++;

    parseParam(rcvdata);


    saveDataToFile();

    //显示状态
    QString sInfo = getFrameStatus(rcvdata);
    _pShowStatusLabel->setText(sInfo);


    _xdatalist.append(_i32FrameCnt*M_FRAME_TIME);
    _yposlist[0].append(_lfPosSetdata);
    _yposlist[1].append(_lfPosRespdata);

    _yspeedlist[0].append(_lfSpeedSetdata);
    _yspeedlist[1].append(_lfSpeedRespdata);

    _ycurrentlist[0].append(_lfQSetdata);
    _ycurrentlist[1].append(_lfQResponsedata);

    _ycurrentAlist.append(_lfACurrentdata);
    _ycurrentBlist.append(_lfBCurrentdata);

}
